#include "RobotController.h"
#include "finishingGroupAction.h"
#include "targetFarGroupAction.h"
#include "AssesmentGroupAction.h"
#include "targetNearGroupAction.h"
#include "AlignGroupAction.h"
#include "AvantiGroupAction.h"
#include "GiraGroupAction.h"
using namespace std;

RobotController::RobotController(ArRobot* robot){
	this->robot = robot;
	align = false;
	thPort = 2100;
	controllerV = new VisioneController();
	angolo=0;
	myState = INIT;
	myOldState = INIT;
}

RobotController::~RobotController(){
	delete(finishing);
	delete(targetfar);
	delete(targetnear);
	delete(scanning);
}
void RobotController::run(){
	pthread_create(&thread,0,&RobotController::start,this);
};

void RobotController::stop(){
	pthread_join(thread,0);
};

void RobotController::dowork()
{
	bool sca = true;
	bool far = true;
	bool flagVisione = false;
	int angoloPalla;
	int posizione = NULL;
	int avanti = 0;
	int gira = 0;
	int tempo=0;
	unsigned short lastPortDepth = 0;
	int posizioneScanning = 1;

	bool tt = false;
	controllerV->alza();
	while(true){

		controllerV->elaborateImage(flagVisione);

		unsigned short portDepth = controllerV->getProfonditaPorta();
		unsigned short ballDepth = controllerV->getProfonditaPalla();

		if(myState!=FINISHING){
			if(portDepth==NULL && sca==true){
				myState = SCANNING;
			}else{
				angolo = controllerV->getAngoloPorta();
				posizioneScanning = angolo<0?1:-1;

				if(portDepth > thPort && far==true){
					myState = TARGETFAR;
				}else if((portDepth<= thPort && portDepth>1) || myOldState==TARGETNEAR
						|| myOldState==ASSESMENT){
					if(myState!=FINISHING && myState!=ALIGN && myState!=ASSESMENT)
						myState = TARGETNEAR;
					unsigned short profPort = controllerV->getProfonditaPorta();
					flagVisione = true;
					sca=false;
					far=false;
					if (ballDepth!=NULL){
						angoloPalla = controllerV->getAngoloPalla();
						if(angoloPalla>=-5 && angoloPalla<=5){
							controllerV->azzera();
							myState = (angolo>=-20 && angolo<=20)?FINISHING:ALIGN;
						}else{
							int angoloPorta = controllerV->getAngoloPorta();
							posizione = angoloPorta>angoloPalla?1:-1;
							myState= ASSESMENT;
						}
					}
				}
			}
		}
		if(myState==SCANNING /*&& myOldState!=SCANNING*/){
			//controllerV->alza();
			//cout << "SONO NELLO STATO SCANNING\n";
			if(avanti<100 && gira==0){
				AvantiGroupAction *av = new AvantiGroupAction(robot);
				av->activateExclusive();
				avanti = 100;
				cout << "AVANZO\n";
			}
			gira++;

			if(gira>=50 && tempo==0){
				GiraGroupAction *gr = new GiraGroupAction(robot,posizioneScanning);
				gr->activateExclusive();
				cout << "GIRO\n";
				tt = true;
			}
			if(tt)
				tempo++;

			if(tempo>=50){
				avanti=0;
				tempo =0;
				gira=0;
				tt=false;
			}

			myOldState = SCANNING;
		}else if(myState==TARGETFAR && myOldState!=TARGETFAR){
			cout << "SONO NELLO STATO FAR\n";
			targetfar= new TargetFarGroupAction(robot,controllerV,portDepth,angolo);
			targetfar->activateExclusive();

			myOldState = TARGETFAR;

		}else if(myState==TARGETNEAR && myOldState!=TARGETNEAR){
			//controllerV->abbassa();
			targetnear= new TargetNearGroupAction(robot,angolo);
			cout << "SONO NELLO STATO TARGETNEAR\n";
			targetnear->activateExclusive();
			myOldState = TARGETNEAR;
		}else if(myState==ASSESMENT && myOldState!=ASSESMENT){

			cout << "MI CENTRO CON LA PALLA\n";
			AssesmentGroupAction *assGroup = new AssesmentGroupAction(robot,controllerV->getAngoloPalla());
			assGroup->activateExclusive();
			myOldState=ASSESMENT;
		}else if(myState == ALIGN && myOldState!=ALIGN){
			cout <<"MI ALLINEO CON LA PALLA"<<endl;
			AlignGroupAction *ali1 = new AlignGroupAction(robot,controllerV,lastPortDepth);
			ali1->activateExclusive();
			ArUtil::sleep(1000);
			myState = FINISHING;
			controllerV->abbassa();
			ArUtil::sleep(3500);
			myOldState = ALIGN;
		}else if (myState==FINISHING && myOldState!=FINISHING){
			cout<<"SONO NELLO STATO FINISHING\n";
			beta = 5;
			finishing= new FinishingGroupAction(robot,posizione,controllerV,&beta);
			finishing->activateExclusive();
			myOldState= FINISHING;
		}
	}

}
void RobotController::dowork_final(){


}

